import cv2
from datetime import datetime
from .cv_hough_line import detecting_lane

# cv2.putText(frame, Text, org, font, color, thickness)

def add_time_to_image(frame):
    now = datetime.now() 
    time = now.strftime("%H:%M:%S.%f")[:-3]
    font=cv2.FONT_HERSHEY_SIMPLEX
    cv2.putText(frame, "Time:"+time,
                          (200, 100), font,1,(0, 255, 255),2,cv2.LINE_4)
    return frame  

def add_servo_angle_to_image(frame,dual_axis):
    font=cv2.FONT_HERSHEY_SIMPLEX
    cv2.putText(frame, "The Angle of",
                        (300, 150), font,1,(65,105,225),2,cv2.LINE_4)
    cv2.putText(frame, "  Vertical Servo:"+str(dual_axis.vertical_servo.current_angle),
                            (300, 200), font,1,(0, 255, 255),2,cv2.LINE_4)
    cv2.putText(frame, "  Horizontal Servo:"+str(dual_axis.horizontal_servo.current_angle),
                            (300, 250), font,1,(0, 255, 255),2,cv2.LINE_4)    
    return frame  

def add_detectedlane_image(frame):
    results= detecting_lane(frame)
    #firstSquareCenters1, firstSquareCenters2,centers,laneCenters, result, mainCenterPosition= detectedlane1(frame)
    if  results is not None:
        font=cv2.FONT_HERSHEY_SIMPLEX
        maincenter=results[5]
        cv2.putText(frame, "Pos="+str(maincenter),
                    (200, 400), font,1,(0, 255, 0),2,cv2.LINE_4)
       
        car_run_text="Action: "
        car_location_text="Location: "
       
        if maincenter <= 6 and maincenter > -6:
           car_run_text=car_run_text+"Forward"
           car_location_text=car_location_text+"Middle"
        elif maincenter > 6 :
           car_run_text=car_run_text+"Turn Left"
           car_location_text=car_location_text+"Right"
        elif maincenter < -6:
           car_run_text=car_run_text+"Turn Right"
           car_location_text=car_location_text+"Left"
     
        cv2.putText(frame, car_location_text,
                    (200, 450), font,1,(0, 255, 0),2,cv2.LINE_4)
        cv2.putText(frame, car_run_text,
                    (200, 500), font,1,(0, 255, 0),2,cv2.LINE_4)
       
        x_factor=int(1920/320)
        y_factor=int(1080/240)
        firstSquareCenters1=(results[0][0]*x_factor,results[0][1]*y_factor)
        firstSquareCenters2=(results[1][0]*x_factor,results[1][1]*y_factor)
        frame=cv2.circle(frame,(int(1920/2),int(1080/2)),10,(0,0,255),-1)
        cv2.line(frame, firstSquareCenters1, firstSquareCenters2, (0, 255, 0), 1)
       
        centers=results[2]
        if centers is not None:
           centers[0]=(centers[0][0]*x_factor,centers[0][1]*y_factor)
           centers[1]=(centers[1][0]*x_factor,centers[1][1]*y_factor)
           cv2.line(frame, centers[0], centers[1], [0, 255, 0], 2)
        
        # add edge lines to the origal image
        pts1 = [[0, 1080], [1920, 1080], [1880, 30], [30, 30]]
        cv2.line(frame, (pts1[0][0],pts1[0][1]), (pts1[1][0],pts1[1][1]), (0, 255, 0), 2)
        cv2.line(frame, (pts1[1][0],pts1[1][1]), (pts1[2][0],pts1[2][1]), (0, 255, 0), 2)
        cv2.line(frame, (pts1[2][0],pts1[2][1]), (pts1[3][0],pts1[3][1]), (0, 255, 0), 2)
        cv2.line(frame, (pts1[3][0], pts1[3][1]), (pts1[0][0], pts1[0][1]), (0, 255, 0), 1)
   
        return  frame
    else:
        return  frame


def add_object_feature_image(frame):
    font=cv2.FONT_HERSHEY_SIMPLEX
    cv2.putText(frame, "Object Feature",
                            (500, 50), font,1,(0, 255, 255),2,cv2.LINE_4)
    return frame  
        
